#include "stdafx.h"
#include "../Engine/AnimationController.hpp"

namespace he
{
	namespace
	{
		//! Helper plane.
		Ogre::Plane g_zx_plane(Ogre::Vector3::UNIT_Y, Ogre::Vector3::UNIT_Z);
	}

	AnimationController::AnimationController(std::string const &Animation, Ogre::Entity *pEntity, Ogre::SceneNode *pSceneNode, bool ControlMove, bool ControlRotation, bool Enabled)
		: m_pRootBone(*pEntity->getSkeleton()->getRootBoneIterator().begin()),
		m_pAnimation(pEntity->getSkeleton()->getAnimation(Animation)),
		m_pAnimationState(pEntity->getAnimationState(Animation)),
		m_pAnimationTrack(m_pAnimation->getNodeTrack(m_pRootBone->getHandle())),
		m_pSceneNode(pSceneNode),
		m_ControlMove(ControlMove),
		m_ControlRotation(ControlRotation),
		m_LastTimePosition(0)
	{
		// Reset animation state
		m_pAnimationState->setTimePosition(0);
		m_pAnimationState->setEnabled(Enabled);
		m_pAnimationState->setLoop(false);
		// Need to set that root bone is manually controller
		m_pAnimationState->createBlendMask(pEntity->getSkeleton()->getNumBones());
		m_pAnimationState->setBlendMaskEntry(m_pRootBone->getHandle(), 0);
		m_pRootBone->setManuallyControlled(true);
		// Get the last position for given animation
		m_pRootBone->reset();
		m_pAnimationTrack->applyToNode(m_pRootBone, m_pAnimation->_getTimeIndex(m_pAnimationState->getLength()));
		m_LastPosition = g_zx_plane.projectVector(m_pRootBone->getPosition());
		// Initialize animation and get the init positions
		m_pRootBone->reset();
		m_pAnimationTrack->applyToNode(m_pRootBone, m_pAnimation->_getTimeIndex(m_pAnimationState->getTimePosition()));
		m_StartPostion = g_zx_plane.projectVector(m_pRootBone->getPosition());
		m_PreviousPosition = m_StartPostion;

		m_PreviousAxis = m_pRootBone->getLocalAxes().GetColumn(0);
		m_PreviousAxis.y = 0;
		m_PreviousAxis.normalise();
	}

	void AnimationController::update(float Delta)
	{
		m_pAnimationState->addTime(Delta);
		// Need to reset because it values are added
		m_pRootBone->reset();
		// Get the position of pelvis bone								
		m_pAnimationTrack->applyToNode(m_pRootBone, m_pAnimation->_getTimeIndex(m_pAnimationState->getTimePosition()));
		Ogre::Vector3 root_bone_position = m_pRootBone->getPosition();
		// Position delta
		Ogre::Vector3 d_position = Ogre::Vector3::ZERO;
		// Rotation delta
		Ogre::Quaternion d_rotation;
		// Project bone position to ZX plane
		Ogre::Vector3 current_position = g_zx_plane.projectVector(root_bone_position);
		// Get pelvis right axis
		Ogre::Vector3 current_axis_local = m_pRootBone->getLocalAxes().GetColumn(0);
		current_axis_local.y = 0;
		current_axis_local.normalise();

		Ogre::Vector3 p;

		if(m_pAnimationState->getTimePosition() - m_LastTimePosition < 0)
			p = m_pAnimationState->getWeight() *((m_LastPosition - m_PreviousPosition) + (current_position - m_StartPostion));
		else
			p = m_pAnimationState->getWeight() * (current_position - m_PreviousPosition);

		// Rotate displacement into current orientation
		// Compute angular displacement and append to result
		if(m_ControlRotation)
		{
			p = current_axis_local.getRotationTo(Ogre::Vector3::UNIT_X)  * p;
			d_rotation = Ogre::Quaternion::Slerp(m_pAnimationState->getWeight(), Ogre::Quaternion::IDENTITY, m_PreviousAxis.getRotationTo(current_axis_local), true);
		}
		if(m_ControlMove)
		{
			d_position += p;
			d_position = Ogre::Vector3::UNIT_Z * d_position.dotProduct(Ogre::Vector3::UNIT_Z);
		}
		// Need to clear "z" position of root node
		m_pRootBone->setPosition(root_bone_position.x, root_bone_position.y, 0);

		if(m_ControlRotation)
		{
			m_pRootBone->setOrientation(current_axis_local.getRotationTo(Ogre::Vector3::UNIT_X) * m_pRootBone->getOrientation());
			m_pSceneNode->rotate(d_rotation);
		}

		if(m_ControlMove)
			m_pSceneNode->translate(d_position, Ogre::SceneNode::TS_LOCAL);
		
		// Update position, orientation
		m_LastTimePosition = m_pAnimationState->getTimePosition();
		m_PreviousPosition = current_position;
		m_PreviousAxis = current_axis_local;
	}

	bool AnimationController::has_ended() const
	{
		return m_pAnimationState->hasEnded();
	}
} /*he*/
